#include "../DWMRICore/Scalar.h"


#include <float.h>

#define _USE_MATH_DEFINES
#include <math.h>

#define P_SCALE 4

char *src_pathname = NULL;
char *dst_pathname = NULL;

void PrintUsage()
{
	printf("Usage: ComputeDistanceVolume -src <src_file> -dst <dst_file>\n");
}



int ParseArguments(const int argc, char *argv[])
{
	for (int i = 1; i < argc; ++i) {
		if (strcmp(argv[i], "-src") == 0) {
			src_pathname = argv[++i];
		} else if (strcmp(argv[i], "-dst") == 0) {
			dst_pathname = argv[++i];
		} else {
			return -1;
		}
	}

	if (dst_pathname == NULL || dst_pathname == NULL)
		return -2;

	return 0;
}

void ComputeDistanceVolume_NEW(CScalar *pSRC, float *res, 
	const int length, const int w, const int h, const int d)
{
	for (int iz = 0; iz < d; ++iz) {
		for (int iy = 0; iy < h; ++iy) {
			for (int ix = 0; ix < w; ++ix) {
				int index = (iz * h + iy) * w + ix;
				if (pSRC->GetScalarFast(ix, iy, iz) > 0.5f)
					res[index] = FLT_MAX;
				else
					res[index] = 0.0f;
			}
		}
	}
	
	printf("\n");
	int count = 0;
	for (int iz = 0; iz < d; ++iz) {
		for (int iy = 0; iy < h; ++iy) {
			for (int ix = 0; ix < w; ++ix) {
				count++;

				float value = pSRC->GetScalarFast(ix, iy, iz);
				if (value > 0.5f) {
					continue;
				}

				int min_x = ix - P_SCALE * length > 0 ? (ix - P_SCALE * length) : 0;
				int max_x = ix + P_SCALE * length < w ? (ix + P_SCALE * length) : (w - 1);
				int min_y = iy - P_SCALE * length > 0 ? (iy - P_SCALE * length) : 0;
				int max_y = iy + P_SCALE * length < h ? (iy + P_SCALE * length) : (h - 1);
				int min_z = iz - P_SCALE * length > 0 ? (iz - P_SCALE * length) : 0;
				int max_z = iz + P_SCALE * length < d ? (iz + P_SCALE * length) : (d - 1);

				for (int jz = min_z; jz <= max_z; ++jz) {
					for (int jy = min_y; jy <= max_y; ++jy) {
						for (int jx = min_x; jx <= max_x; ++jx) {
							//if (pSRC->GetScalarFast(jx, jy, jz) < 0.5f) {
							//	continue;
							//}

							float dx = float(ix - jx);
							float dy = float(iy - jy);
							float dz = float(iz - jz);

							float dis = (dx * dx + dy * dy + dz * dz);

							int index = (jz * h + jy) * w + jx;

							if (dis < res[index]) {
								res[index] = dis;
							}
						}
					}
				}
			}
		}
		float ff = float(count + 1) / float(w * h * d) * 100.0f;
		printf("\rfinish %.3f%%", ff);
	}

	printf("\rfinish %.3f%%\n", 100.0f);
}

void ComputeDistanceVolume_KDE(CScalar *pSRC, float *res, 
	const int length, const int w, const int h, const int d)
{
	float p1 = 1.0f / (length * sqrt(2.0f * M_PI));
	float p2 = 2.0f * length * length;

	memset(res, 0, sizeof(float)*w*h*d);

	printf("\n");
	int count = 0;
	for (int iz = 0; iz < d; ++iz) {
		for (int iy = 0; iy < h; ++iy) {
			for (int ix = 0; ix < w; ++ix) {
				float value = pSRC->GetScalarFast(ix, iy, iz);

				int min_x = ix - P_SCALE * length > 0 ? (ix - P_SCALE * length) : 0;
				int max_x = ix + P_SCALE * length < w ? (ix + P_SCALE * length) : (w - 1);
				int min_y = iy - P_SCALE * length > 0 ? (iy - P_SCALE * length) : 0;
				int max_y = iy + P_SCALE * length < h ? (iy + P_SCALE * length) : (h - 1);
				int min_z = iz - P_SCALE * length > 0 ? (iz - P_SCALE * length) : 0;
				int max_z = iz + P_SCALE * length < d ? (iz + P_SCALE * length) : (d - 1);

				for (int jz = min_z; jz <= max_z; ++jz) {
					for (int jy = min_y; jy <= max_y; ++jy) {
						for (int jx = min_x; jx <= max_x; ++jx) {
							float dx = float(ix - jx);
							float dy = float(iy - jy);
							float dz = float(iz - jz);
							//float dis = -1.0f * (dx * dx + dy * dy + dz * dz);
							//float p = p1 * expf(dis / p2);
							
							float dis = (dx * dx + dy * dy + dz * dz);
							float p = dis;
							
							p = p * value;
							uint index = (jz * h + jy) * w + jx;

							res[index] += p;
							/*if (res[index] < p) {
								res[index] = p;
							}*/
						}
					}
				}

				count++;
			}
		}
		float ff = float(count + 1) / float(w * h * d) * 100.0f;
		printf("\rfinish %.3f%%", ff);
	}

	printf("\rfinish %.3f%%\n", 100.0f);
}


int main(int argc, char *argv[])
{
	if (ParseArguments(argc, argv) != 0) {
		PrintUsage();
		return 0;
	}

	CScalar *pSRC = new CScalar();
	pSRC->OpenScalarFile(src_pathname);

	uint w = pSRC->GetWidth();
	uint h = pSRC->GetHeight();
	uint d = pSRC->GetDepth();

	float *distance = new float[w*h*d];
	int length = (w + h + d) / 30;
	length = 3;
	//ComputeDistanceVolume_KDE(pSRC, distance, length, w, h, d);
	ComputeDistanceVolume_NEW(pSRC, distance, 7, w, h, d);

	CScalar *pDST = new CScalar();
	pDST->CreateScalar(w, h, d, distance);
	pDST->SaveScalarFile(dst_pathname);

	SafeDelete(pSRC);
	SafeDelete(pDST);
	SafeDeleteArray(distance);

	return 0;
}